//
// Created by 张祎鹤严 on 2023/9/24.
//

#ifndef DRIVER_VISION_H
#define DRIVER_VISION_H

#include "bsp_usb.h"
#include "math.h"
#include "pid_driver.h"
#include "kalman_filter.h"
#include "arm_math.h"
#include "bsp_dwt.h"
#include "pose_task.h"
#include "pid_driver.h"
#include "shoot_task.h"

#define Sentry_Left_Gimbal 1//
#define USE_GYRO 1
#define FORCE_NOT_FIRE 0

#define PITCH_GYRO_INIT_VALUE_SET (0.0f)

#define GRAVITY (9.7911f)
#define RAD2DEG  (180/PI)             //180/PI
#define DEFAULT_K_42  (0.035544 / 4)       //42mm弹丸的初始k值 ///0.017772f;z
#define DEFAULT_K_17 (0.08334 / 4)       //17MM弹丸的初始k值
#define Color_BR_Diff_Val 5 //status diff btw red/blue
#define USB_Buff_Size	64
#define TIME_ALIGN_BUFFER_SIZE 64
#define VISION_NEW 1
#define FLIGHTTIME (540)
#define GimbalReacTime (400)
#define Dt (0.001f)
/***********
			该程序从视觉处拷来，具体源代码参照视觉组
			*********************************////

#if Sentry_Left_Gimbal
//动态
#define VISION_PITCH_KP (0.0f)
#define VISION_PITCH_KI (-0.5f)
#define VISION_PITCH_KD (0.0f)
#define VISION_PITCH_AP (0.012f)//0.005
#define VISION_PITCH_BP (0.1f) //0.0
#define VISION_PITCH_CP (1.0f) //1.0

#define VISION_YAW_KP (0.0f)
#define VISION_YAW_KI (0.0f)
#define VISION_YAW_KD (0.0f)
#define VISION_YAW_AP (-0.001f)//(-0.0005f)//(-0.0008f)
#define VISION_YAW_BP (0.02f)//(0.0f)
#define VISION_YAW_CP (-0.005f)//(0.08)//(-0.002f)//(1.0f)

//这里坐标系定义为右x，前y，上z
#define OFFSET_X 			(0.0)//摄像头中心和摩擦轮中心的相对偏置
#define OFFSET_Y 			(88.0)
#define OFFSET_Z 			(58.0)
///**************偏置和空气阻力***************///
#define VISION_V 			(0)
#define VISION_K 			(0.1f)//空气阻力


#define VISION_YAW 		(0.f)//(0.4f)// - right  + left
#define VISION_PITCH 	(1.0f)

#define VISION_K_18 			(0.1f)
#define VISION_YAW_18 		(0.9f)// - right  + left
#define VISION_PITCH_18 	(3.f)

#define VISION_K_30 			(0.1f)
#define VISION_YAW_30 		(0.9f)// - right  + left
#define VISION_PITCH_30 	(3.f)

#define ENERGY_OFFSET_UP (-0.012f)
#define ENERGY_OFFSET_DOWM (0.f)
#define SHOOT_DELAY_TIME (100)

#define ENERGY_PITCH_AP (0.12f)
#define ENERGY_YAW_AP (0.06f)

#define VISIONMAXYAW 6.0f
#define VISIONMAXPITCH 5.0f

#elif Yellow_Duck
//动态
#define VISION_PITCH_KP (0.0)
#define VISION_PITCH_KI (0.0)
#define VISION_PITCH_KD (0.0f)
#define VISION_PITCH_AP (0.01f)
#define VISION_PITCH_BP (0.0f)
#define VISION_PITCH_CP (1.0f)

#define VISION_YAW_KP (0.0)
#define VISION_YAW_KI (0.0)
#define VISION_YAW_KD (0.0f)
#define VISION_YAW_AP (-0.0015f)
#define VISION_YAW_BP (0.0f)
#define VISION_YAW_CP (1.0f)

#define OFFSET_X 			(0.0)//摄像头中心和摩擦轮中心的相对偏置
#define OFFSET_Y 			(55.0)
#define OFFSET_Z 			(68.0)

#define VISION_V 			(0)
#define VISION_K 			(0.1f)
#define VISION_YAW 		(0.6f)// - right  + left
#define VISION_PITCH 	(0.f)

#define VISION_K_18 			(0.1f)
#define VISION_YAW_18 		(0.9f)// - right  + left
#define VISION_PITCH_18 	(3.f)

#define VISION_K_30 			(0.1f)
#define VISION_YAW_30 		(0.9f)// - right  + left
#define VISION_PITCH_30 	(3.f)

#define ENERGY_OFFSET_UP (0.004f)
#define ENERGY_OFFSET_DOWM (0.f)
#define SHOOT_DELAY_TIME (100)

#define ENERGY_PITCH_AP (0.12f)
#define ENERGY_YAW_AP (0.06f)

#elif Old_Infantry
#define VISION_PITCH_KP (0.0)
#define VISION_PITCH_KI (0.0)
#define VISION_PITCH_KD (2.f)
#define VISION_PITCH_AP (0.0045f)
#define VISION_PITCH_BP (0.f)
#define VISION_PITCH_CP (0.6f)

#define VISION_YAW_KP (0.0)
#define VISION_YAW_KI (0.0)
#define VISION_YAW_KD (4.f)
#define VISION_YAW_AP (-0.0012f)
#define VISION_YAW_BP (0.002f)
#define VISION_YAW_CP (0.8f)

#define OFFSET_X 			(0.0)//摄像头中心和摩擦轮中心的相对偏置
#define OFFSET_Y 			(55.0)
#define OFFSET_Z 			(68.0)

#define VISION_V 			(0)
#define VISION_K 			(0.01f)
#define VISION_YAW 		(0.f)// - right  + left
#define VISION_PITCH 	(3.f)

#define VISION_K_18 			(0.4f)
#define VISION_YAW_18 		(0.0f)// - right  + left
#define VISION_PITCH_18 	(4.f)

#define VISION_K_30 			(0.4f)
#define VISION_YAW_30 		(0.0f)// - right  + left
#define VISION_PITCH_30 	(1.2f)

#define ENERGY_OFFSET_UP (0.0085)//高+矮-
#define ENERGY_OFFSET_DOWM (0.013f)//高+矮-
#define SHOOT_DELAY_TIME (80)

#define ENERGY_PITCH_AP (0.12f)
#define ENERGY_YAW_AP (0.06f)

#elif Steer_Infantry
#define VISION_PITCH_KP (0.0)///自瞄响应
#define VISION_PITCH_KI (0.0)
#define VISION_PITCH_KD (5.0f)
#define VISION_PITCH_AP (0.006f)
#define VISION_PITCH_BP (0.0f)
#define VISION_PITCH_CP (1.0f)

#define VISION_YAW_KP (0.0)
#define VISION_YAW_KI (0.0)
#define VISION_YAW_KD (0.0f)
#define VISION_YAW_AP (-0.0015f)
#define VISION_YAW_BP (0.f)
#define VISION_YAW_CP (1.0f)

#define OFFSET_X 			(0.0)//摄像头中心和摩擦轮中心的相对偏置
#define OFFSET_Y 			(55.0)
#define OFFSET_Z 			(68.0)

#define VISION_V 			(0)///弹道偏置
#define VISION_K 			(0.1f) //空气阻力系数
#define VISION_YAW 		(0.6f)// - right  + left
#define VISION_PITCH 	(2.f)

#define VISION_K_18 			(0.1f)
#define VISION_YAW_18 		(0.9f)// - right  + left
#define VISION_PITCH_18 	(3.f)

#define VISION_K_30 			(0.1f)
#define VISION_YAW_30 		(0.9f)// - right  + left
#define VISION_PITCH_30 	(3.f)

#define ENERGY_OFFSET_UP (0.f)
#define ENERGY_OFFSET_DOWM (0.f)
#define SHOOT_DELAY_TIME (100)

#define ENERGY_PITCH_AP (0.16f)
#define ENERGY_YAW_AP (0.06f)

#elif NEW_INFANTRY
#define VISION_PITCH_KP (0.0f)
#define VISION_PITCH_KI (-0.5f)
#define VISION_PITCH_KD (0.0f)
#define VISION_PITCH_AP (0.01f)//0.005
#define VISION_PITCH_BP (0.1f) //0.0
#define VISION_PITCH_CP (1.0f) //1.0

#define VISION_YAW_KP (0.0f)
#define VISION_YAW_KI (0.0f)
#define VISION_YAW_KD (0.0f)
#define VISION_YAW_AP (-0.0007f)//(-0.0005f)//(-0.0008f)
#define VISION_YAW_BP (0.001f)//(0.0f)
#define VISION_YAW_CP (-0.002f)//(0.08)//(-0.002f)//(1.0f)

#define OFFSET_X 			(0.0)//摄像头中心和摩擦轮中心的相对偏置
#define OFFSET_Y 			(55.0)
#define OFFSET_Z 			(68.0)
///**************偏置和空气阻力***************///
#define VISION_V 			(0)
#define VISION_K 			(0.1f)//空气阻力
#define VISION_YAW 		(0.0f)// - right  + left
#define VISION_PITCH 	(1.0f)

#define VISION_K_18 			(0.1f)
#define VISION_YAW_18 		(0.9f)// - right  + left
#define VISION_PITCH_18 	(3.f)

#define VISION_K_30 			(0.1f)
#define VISION_YAW_30 		(0.9f)// - right  + left
#define VISION_PITCH_30 	(3.f)

#define ENERGY_OFFSET_UP (0.0f)
#define ENERGY_OFFSET_DOWM (0.f)
#define SHOOT_DELAY_TIME (100)

#define ENERGY_PITCH_AP (0.12f)
#define ENERGY_YAW_AP (0.06f)

#define VISIONMAXYAW 15.0f
#define VISIONMAXPITCH 5.0f

#define TRANSFORMDELAYTIME 0.2f
#elif Balance_Infantry
#define VISION_PITCH_KP (0.0f)
#define VISION_PITCH_KI (-0.5f)
#define VISION_PITCH_KD (0.0f)
#define VISION_PITCH_AP (0.012f)//0.005
#define VISION_PITCH_BP (0.1f) //0.0
#define VISION_PITCH_CP (1.0f) //1.0

#define VISION_YAW_KP (0.0f)
#define VISION_YAW_KI (0.0f)
#define VISION_YAW_KD (0.0f)
#define VISION_YAW_AP (-0.001f)//(-0.0005f)//(-0.0008f)
#define VISION_YAW_BP (0.02f)//(0.0f)
#define VISION_YAW_CP (-0.005f)//(0.08)//(-0.002f)//(1.0f)

#define OFFSET_X 			(0.0)//摄像头中心和摩擦轮中心的相对偏置
#define OFFSET_Y 			(55.0)
#define OFFSET_Z 			(68.0)
///**************偏置和空气阻力***************///
#define VISION_V 			(0)
#define VISION_K 			(0.1f)//空气阻力
#define VISION_YAW 		(0.0f)//(0.4f)// - right  + left
#define VISION_PITCH 	(0.0f)

#define VISION_K_18 			(0.1f)
#define VISION_YAW_18 		(0.9f)// - right  + left
#define VISION_PITCH_18 	(3.f)

#define VISION_K_30 			(0.1f)
#define VISION_YAW_30 		(0.9f)// - right  + left
#define VISION_PITCH_30 	(3.f)

#define ENERGY_OFFSET_UP (0.004f)
#define ENERGY_OFFSET_DOWM (0.f)
#define SHOOT_DELAY_TIME (100)

#define ENERGY_PITCH_AP (0.12f)
#define ENERGY_YAW_AP (0.06f)

#define VISIONMAXYAW 6.0f
#define VISIONMAXPITCH 15.0f

#define TRANSFORMDELAYTIME (0.4f)
#endif

const uint8_t HIGH_SHOOT_FREQ = 13;
const uint8_t MID_SHOOT_FREQ = 14;
const uint8_t LOW_SHOOT_FREQ = 15;

const float RANGE_SCOPE = 5.f;

typedef struct{
    float SmallBulletSpeed_Last;
    float SmallBulletSpeed;
    float SmallBulletSpeedFiltered;
    float SmallBulletSpeedLimit;
}BulletSpeed;

typedef struct {
    uint8_t color_flag;          //red:0 blue:1,自己的颜色
    float bullet_actual_speed;   //读裁判系统,测试给个28
    float bullet_limit_speed;    //始终=30
}AutoAimingParaStruct;

typedef struct
{
    float dist;
    float y_temp;
    float y_actual;
    float y_pos;
    float pitch_target;
    float pitch_change;
    float pitch_temp;
}PNP_Get_Pitch_Struct;

typedef struct
{
    uint8_t AutoAim;
    uint8_t AutoEnergy;
    uint8_t AutoAimStatus;
    uint8_t SymbolShootPermit;
}AimAutoStruct;

typedef struct{
    float offset_x;
    float offset_y;
    float offset_z;
    float init_k;
    float init_v;
    float offset_yaw;
    float offset_pitch;
    float offset_energy_pitch_up;
    float offset_energy_pitch_down;
    float shoot_delay;
}GimbalCtrl;

typedef struct
{
    float horizonal_x;
    float vertical_y;
    float ser_z;
    float hor_x_last;
    float ver_y_last;
    float ser_z_last;
    float delaytime;
    float delaytime_last;
    float speed_x;
    float speed_y;
    uint8_t status;
    float ItTime;
    int DeltaTime;
    float change_pitch;
    float change_yaw;
    float change_pitch_last;
    float change_yaw_last;
    int vision_zero_flag;
    int spin_tower_hit_shoot_enable;
    uint32_t time_stamp;
    uint32_t time_pre;
}VisionReceiveDataStruct;

#pragma pack(1)
typedef struct{
    uint8_t head;//0X71
    float horizon_x;
    float vertical_y;
    uint8_t armor_number;
    uint16_t ser_z;//弹速*10
    uint8_t firing_rate;
    //打符信息
    uint32_t pre_time_stamp;
    //装甲板世界坐标
    float armor_axis_x;
    float armor_axis_y;
    float armor_axis_z;
    //车中心点世界坐标
    float robot_axis_x;
    float robot_axis_y;
    float robot_axis_z;
    float yaw_delta;
    //车中心点速度
    float robot_axis_x_speed;
    float robot_axis_y_speed;
    float robot_axis_z_speed;
    float robot_omega;//车自旋角速度
    uint16_t crc;
    uint8_t tail;//0X4C
}VisionData;
#pragma pack()

typedef union {
    VisionData vision_data;
    uint8_t data[64];
}VisionDataStruct;


typedef struct{
    float x;
    float y;
    float z;
}Point3fStruct;

typedef struct{
    float x;
    float y;
}Point2fStruct;

typedef struct{
    Point3fStruct Axis_Camera;
    Point3fStruct Axis_Horizontal;
    Point3fStruct Axis_World;
    Point3fStruct Axis_Camera_last;
    Point3fStruct Axis_Horizontal_last;
    Point3fStruct Axis_World_last;
    Point3fStruct Axis_tmp1;
    Point3fStruct Axis_tmp2;
    float X_Rotate_Angle;
    float Y_Rotate_Angle;
    float Z_Rotate_Angle;
}Coordinate;

extern VisionDataStruct VisionTransmit;


extern AimAutoStruct AimAutoData;
extern GimbalCtrl VisionGimbalCtrl;
extern VisionDataStruct VisionReceive;


void TransmitData(VisionDataStruct *VisionTransmitDataTemp, uint8_t *UsbTxBufTemp);
void ReceiveData(const uint8_t *Buffer);

void AimAuto(void);

void Transform(Point3fStruct position, float inPitch, float inYaw, float* pitch, float* yaw,
               float* delay, float trans_vel, float forward_vel);//FOWARD是子弹速度+前进分量

void VisionInit(void);

#endif //DRIVER_VISION_H

